#ifndef PHYSICSWORLD_H
#define PHYSICSWORLD_H

#include <ode/ode.h>
#include <vector>
#include <map>
#include <QObject>
#include <QDebug>
#include "Macro.h"
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include "Trajectory.h"
#include "Random"

using namespace Eigen;
using namespace std;

class Joint
{
public:
	dJointID id;
	Vector3d pos;
	Quaternion<double> quat;
	dJointType type;
	Vector3d axis;
	Joint(){};
	Joint(dJointID id, Vector3d& pos, Quaternion<double>& quat, Vector3d& axis, dJointType type)
	{
		this->id = id;
		this->pos = pos;
		this->quat = quat;
		this->axis = axis;
		this->type = type;
	};
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class Link
{
public:
	dBodyID id;
	Vector3d pos;
	Quaternion<double> quat;
	Link(){};
	Link(dBodyID id, Vector3d& pos, Quaternion<double>& quat)
	{
		this->id = id;
		this->pos = pos;
		this->quat = quat;
	};
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class ContactInfo
{
public:
	Vector3d force;
	Vector3d pos;
	int status;
	ContactInfo(Vector3d& force, Vector3d& pos, int status){this->force = force; this->pos = pos; this->status = status;};
	ContactInfo(){};
	~ContactInfo(){};
	void reset()
	{
		this->force = Vector3d(0.0, 0.0, 0.0);
		this->status = 0;
	};
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class PhysicsWorld
{
public:
	dWorldID world;
	dSpaceID space;
	dJointGroupID contactgroup;
	dJointGroupID mousegroup;
	dJointGroupID armgroup;

	std::map<QString, dGeomID> bodies;
	int bodyNum;

	std::map<dGeomID, dBodyID> geoms;
	int geomNum;

	std::map<QString, dJointID> armJoints;
    int armJointNum;

	dBodyID magicBoxBody;
	dGeomID magicBoxGeom;
	
	dBodyID fingerBody;
	dGeomID fingerGeom;

	dGeomID tableGeom;

	dGeomID groundPlane;

	dBodyID mouseBody1;
	dBodyID mouseBody2;
	dJointID mouseJoint;
	
	dBodyID endEffector;

	dJointID wristJoint;
	dJointID conJoint;
	dContact contact;

	PhysicsWorld();
	~PhysicsWorld();

	dReal timeStep;

	ContactInfo conInfo;

	Trajectory* traj;

	dJointFeedback fbCon;
	dJointFeedback fbWrist;

	void animatePhysics(std::vector<Joint, Eigen::aligned_allocator<Joint>>& joints);
	void processCollisions(dGeomID o1, dGeomID o2);
	void servo(std::vector<Joint, Eigen::aligned_allocator<Joint>>& joints);
	void storePathPoint();
	void storeTotalInfo();
	void storeContactInfo();
	void reset();
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif